#include "main.h"
#include "control.h"
#include "motor.h"

extern ControlChannel channels[4];

//PID motorPID[4] = 
//{
//    {.Kp = 0.75, .Ki = 0.001, .Kd = 0.1, .Max = 500, .Min = -500, .IntegralLimit = 10000, .DeadBand = 2},
//    {.Kp = 0.75, .Ki = 0.001, .Kd = 0.1, .Max = 500, .Min = -500, .IntegralLimit = 10000, .DeadBand = 2},
//    {.Kp = 1.5, .Ki = 0.001, .Kd = 0.1, .Max = 500, .Min = -500, .IntegralLimit = 10000, .DeadBand = 2},
//    {.Kp = 1.5, .Ki = 0.001, .Kd = 0.1, .Max = 500, .Min = -500, .IntegralLimit = 10000, .DeadBand = 2}
//};

PID motorPID[4] = 
{
    {.Kp = 0.65, .Ki = 0.00, .Kd = 0.1, .Max = 500, .Min = -500, .IntegralLimit = 10000, .DeadBand = 2},
    {.Kp = 0.7, .Ki = 0.00, .Kd = 0.1, .Max = 500, .Min = -500, .IntegralLimit = 10000, .DeadBand = 2},
    {.Kp = 1.3, .Ki = 0.00, .Kd = 0.1, .Max = 500, .Min = -500, .IntegralLimit = 10000, .DeadBand = 2},
    {.Kp = 1.3, .Ki = 0.00, .Kd = 0.1, .Max = 500, .Min = -500, .IntegralLimit = 10000, .DeadBand = 2}
};

WheelInfo wheelInfo[4] = 
{
    {.wheelSpeed = 0},
    {.wheelSpeed = 0},
    {.wheelSpeed = 0},
    {.wheelSpeed = 0},
};

int motorSpeed[4] = {0, 0, 0, 0};

double FabsD(double x)
{
    if(x > 0) x = x;
    else if(x <= 0) x = -x;
    return x;
}

void motorSetSpeed(int goalSpeed, int ID)
{
    uint8_t id = ID - 1;
    motorPID[id].Target = goalSpeed;
    motorPID[id].NowIS = motorSpeed[id];
    motorPID[id].Err_last = motorPID[id].Err;
    motorPID[id].Err = motorPID[id].Target - motorPID[id].NowIS;
    
//    if(FabsD(motorPID[id].Err) < motorPID[id].DeadBand) motorPID[id].Err = 0;
    motorPID[id].Err_sum += motorPID[id].Err; 
	motorPID[id].Err_sum = CLAMP(motorPID[id].Err_sum, -motorPID[id].IntegralLimit, motorPID[id].IntegralLimit);
	motorPID[id].P_Out = motorPID[id].Kp * motorPID[id].Err;
	motorPID[id].I_Out = motorPID[id].Ki * motorPID[id].Err_sum;
	motorPID[id].D_Out = motorPID[id].Kd * (motorPID[id].Err - motorPID[id].Err_last); 
	
	motorPID[id].PID_Out = motorPID[id].P_Out + motorPID[id].I_Out + motorPID[id].D_Out;
	motorPID[id].PID_Out = CLAMP(motorPID[id].PID_Out, motorPID[id].Min, motorPID[id].Max);
    
    motorSetPWM(&channels[id], motorPID[id].PID_Out);
}

void wheelsRun(void)
{
    for(int8_t id = 0; id < 4; id++)
    {
        motorSetSpeed(wheelInfo[id].wheelSpeed, id + 1);
    }
}

void carSetSpeed(float VX_W, float VY_W, float VZ_W)
{
    float VX=0.0f;
    float VY=0.0f;
    switch(0)
    {
        case 0:
        {
            VX=VX_W;
            VY=VY_W;
            break;
        }
        case 1:
        {
            VX=-VX_W;
            VY=-VY_W;
            break;
        }
        case 2:
        {
            VX=VY_W;
            VY=-VX_W;
            break;
        }
        case 3:
        {
            VX=-VY_W;
            VY=VX_W;
            break;
        }
    }
    float VZ = VZ_W;
    /*四麦轮速度解算，右下角轮子起始，顺时针id增大*/
    wheelInfo[0].wheelSpeed = VY + VX + VZ;
    wheelInfo[1].wheelSpeed = VY - VX + VZ;
    wheelInfo[2].wheelSpeed = VY + VX - VZ;
    wheelInfo[3].wheelSpeed = VY - VX - VZ;
    
    
//    wheelInfo[0].wheelSpeed = VY + VX - VZ;
//    wheelInfo[1].wheelSpeed = VY - VX + VZ;
//    wheelInfo[2].wheelSpeed = VY + VX + VZ;
//    wheelInfo[3].wheelSpeed = VY - VX - VZ;
    /*驱动电机运动*/
    wheelsRun();
}
